#ifndef ODOMETRY_MODEL_H
#define ODOMETRY_MODEL_H

#include "ros/ros.h"
#include "geometry_msgs/PoseArray.h"
#include "tf/transform_datatypes.h"

#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

class OdomModel
{
public:
	OdomModel(ros::NodeHandle& nh, ros::NodeHandle& ph, boost::mt19937& g);

	~OdomModel(){};

	void updateMotion(geometry_msgs::PoseArray& particles,
					  double prev_x, double prev_y, double prev_yaw,
				   	  double next_x, double next_y, double next_yaw);

	void addNRandParticles(int N, std::vector<geometry_msgs::Pose>& cloud);
private:
	ros::NodeHandle& n;
	ros::NodeHandle& pn;
	double a1, a2, a3, a4;// robot-specific noise parameters

	std::vector<std::pair<double, double> > free_area;
//	boost::mt19937& gen;
	boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > gaussian;
};

/***********************************************
 * Helper functions
 ***********************************************/
inline double normalize_angle(double z)
{
  return atan2(sin(z),cos(z));
}

inline double angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize_angle(a);
  b = normalize_angle(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}
#endif
